Modeling Environments through Range

Scanning

A Senior Project submitted to

The Division of Science, Mathematics, and Computing

Bard College

Erik Shagdar

Annandale-on-Hudson, New York

May, 2011

Abstract

This project explores creating three-dimensional models of environments using a series of

two-dimensional range images. Firstly, available sensor options are surveyed and a three-

dimensional laser range ﬁnder is designed and built using a tilting two-dimensional laser

range ﬁnder. Secondly, data is collected from various environments under diﬀerent scanning

conditions. Finally, the environments are modeled as three-dimensional clouds of points

Contents

Abstract 1

Dedication 6

Acknowledgments 7

1 Introduction 8

1.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.2 Explorations of Moving the Laser . . . . . . . . . . . . . . . . . . . . . . . . 10

2 2D Scanning Options 12

2.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.2 Sensor Module . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.3 Laser Range Scanner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

3 3D Scanner Platform 16

3.1 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3.1.1 Laser Rig . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3.1.2 Bioloid Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3.1.3 Bioloid Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3.1.4 Bioloid Servos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.1.5 Bioloid Sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.1.6 Hokuyo Laser . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3.1.7 Connector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

3.2 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

3.2.1 Bioloid Standard Applications . . . . . . . . . . . . . . . . . . . . . 23

3.2.2 Python Libraries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

Contents 3

4 3D Model Building 25

4.1 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

4.2 Early Results and Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

4.3 Finalized Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

4.4 Scans . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

5 Discussion and future work 45

5.1 Attachment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

5.2 Algorithm Improvement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

Bibliography 47

List of Figures

1.2.1 Arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

1.2.2 Arm and Laser . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.2.1 AX-S1 sensor model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.3.1 Laser sensor model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.1.1 Laser Rig: Laser attached to control module via a servo . . . . . . . . . . . 16

3.1.2 CM-5 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3.1.3 Left: Diﬀerent sides of the servo, Right: Servos serially linked to the CM-5 . 18

3.1.4 Control table of data ranges [2] . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.1.5 Top: Sensor module, Bottom: Close and long range sensing [1] . . . . . . . . 21

3.1.6 Left: Laser range scanner, Right: Measurable range of the scanner . . . . . 22

3.1.7 Plate connector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

3.2.1 System-Level Block Diagram . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4.2.1 Model standing in the middle of the room . . . . . . . . . . . . . . . . . . . 27

4.2.2 Far wall and corner of a room . . . . . . . . . . . . . . . . . . . . . . . . . . 28

4.2.3 Corner of a room leading to two directions . . . . . . . . . . . . . . . . . . . 29

4.2.4 Corner of the sun room . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

4.3.1 Living Room Panorama . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

4.3.2 Model standing in the middle of the room . . . . . . . . . . . . . . . . . . . 32

4.3.3 Scan of the model standing in the middle of the room using the ﬁnalized

algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

4.3.4 Weis Cinema . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

4.3.5 Stairs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

4.4.1 Armchair 10 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

4.4.2 Armchair 4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

LIST OF FIGURES 5

4.4.3 Armchair speed 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

4.4.4 Armchair speed 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

4.4.5 Model speed 10 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

4.4.6 Model speed 4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

4.4.7 Model speed 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

4.4.8 Model speed 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

4.4.9 Weis speed 10 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

4.4.10Weis speed 4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

4.4.11Weis speed 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

4.4.12Weis speed 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

4.4.13stairs Speed 10 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

4.4.14stairs Speed 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

4.4.15stairs Speed 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

4.4.16stairs Speed 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

Dedication

To my grandfather – the man I respect the most.

Acknowledgments

I would like to acknowledge Keith O’Hara for his endless help on this project, without

whom this would have been a rough year. I would also like to thank Sara and Meghan for

being models in the scans.

Introduction

1.1 Overview

Range detection is used in applications varying from military applications, naval nav-

igation, 3D model building, and even map building. Range ﬁnders use the concept of

transmitting a signal, waiting until it hits some obstacle and returns to the source to be

received, and then measuring the time of ﬂight to cover the distance. As technology im-

proved, so did detection – simple scanners became faster, more complex and accurate.

Laser range scanners transmit a pulse of light (typically an infrared laser) and measure

the time diﬀerence between when it was sent and when it was received. Light is used be-

cause its speed is constant. The only variable is the distance to the object, and is measured

by taking half of the product of the time it takes for the pulse to travel to the object and

back. with the speed of light

Most scanners available provide one-dimensional (1D) or two-dimensional (2D) scans.

This work explores three-dimensional (3D) model building by combining a 2D scanner

1. INTRODUCTION 9

with the ability to rotate about the other axis. There are existing 3D scanners available,

such as those produced by Velodyne, which cost about $75,000. The laser used in this

project costs just under $1,200, but with all the servos the total cost becomes $2,100. This

is still a small fraction of the cost used to accomplish the same concept. However, the laser

developed in this project is slower

As Blais mentions in his paper on the review of range sensor development [6], range sens-

ing has made many advancements in recent years. 2D range sensing is the most popular

and eﬃcient way of imaging environments. It is a logical step from single point detec-

tors and oﬀers a much faster, easier, and complete approach to one-dimensional scanning.

These point scanners serve as a basis for establishing a way to optically recognize surfaces.

A laser projection and an imaging lens are used to triangulate the distance of the object

with the use of mirrors. This concept is then extended to a 2D detector to provide planar

projection. With the use of a tilting mechanism, the planar scan can be extended into a

third dimension.

A laser range ﬁnder capturing a scan looks at a 2D surface, and collects all the points

within it’s range of view where it senses the nearest obstacle. Each point, not being able

to know anything about it’s neighbors, stands alone. The nature of the scanner doesn’t

provide any information about what is behind that ﬁrst obstacle point. This combined

with the way 2D scanners are built and measure distances, oﬀer limited information that

decreases as the obstacles move farther away. With the help of a servo, the scanner tilts

and scans a new plane. The problem becomes more interesting when going beyond this

sparse information about points: would it be possible to recreate the environment the

scanner is looking at? Further, given these limited points, is it possible for someone to

infer whether the recreated model looks similar to the environment, or is the loss of some

1. INTRODUCTION 10

information vital to the inability to create models?

The 3D scans will be assessed based on how more detail can be gathered and presented

in a neater and more user friendly way. As shown in the paper, the slower the scans

are, the denser and more detailed the visualizations will be. Given a collection of points

with coordinates- a point cloud, how well can the environment be recreated? Of course

the evaluation is left somewhat to the viewer and their ability to infer what the points

depict. One goal is to have viewers agree that the image accurately conveys the actual

environment.

1.2 Explorations of Moving the Laser

Figure 1.2.1. Arm

The project started with a concentration that diﬀered from the ﬁnal product. The ini-

tial idea was to create a manipulator that would learn about itself and complete tasks.

Learning was done by using forward learning models where the robot would bend each

1. INTRODUCTION 11

joint to see what the eﬀects would be [8]. After this initial step, the manipulator would

scan for obstacles within its range and move them based on what shape it thought they

were.

Figure 1.2.2. Arm and Laser

The manipulator consisted of a series of joints and a grabbing mechanism. The joints

are made up of two servos used to bend the arm. Two servos were needed because a single

servo was too weak to lift the manipulator by itself. Bending of the joints was a trick

of its own. The servos, having a serial connection, could communicate with the central

module one at a time. To overcome this, the two servos alternated moving a small amount

(much like people walk). One servo would lock itself and support the weight of the arm

while the other would move slightly past than the ﬁrst and lock itself. The ﬁrst would

then unlock, move past the ﬁrst, and re-lock. This process was repeated multiple times to

achieve bending at any angle. The servos could move slightly past one another because

of the nature of the construction of the pieces. The combination of the nuts and bolts

holding two pieces together allowed for slight bending between the two pieces. Although

this doesn’t seem like much of a beneﬁt, having three or four pieces being held together

in such a way provided plenty of room for the servos to function as they needed to in

order to lift a heavy object. Although the arm was not used, a much simpler version is

the foundation of the 3D scanning laser range ﬁnder.

2D Scanning Options

2.1 Overview

In this paper, two potential sensors for range ﬁnding are explored. One is the sensor

module that comes with the Bioloid Kit, and the other is Hokuyo’s URG-04LX-UG01

laser range ﬁnder. The laser performed better by getting an accurate and linear result

while the module returned a logarithmic result. Additionally, the laser did not skew much

as the surfaces varied. Ultimately, the laser range scanner was chosen for it’s basic scanning

technology. As evidenced below, it proved to be much more accurate than the IR scanner

and provided more data

2.2 Sensor Module

The AX-S1 is a sensor module that is included with Bioloid’s Comprehensive Kit. The

module is used to interact with the environment via range, light, and noise detection.

Three sides of the sensor module have a range detector that measures the distance to an

object, using values between 0 and 255. Moving an object away from any one of the sensors

results in a value that starts high and drops down to zero for that sensor. The reason for

2. 2D SCANNING OPTIONS 13

this decrease has to do with the way the sensor works. The detector is supplied a voltage

and returns another analog voltage from the receiver. As the distance between the sensor

and obstacle increases, the distance travelled from the transmitter to the object and back

to the receiver also increases. The greater the distance between the two, the bigger the

voltage drop, and so the closer to zero volts received. This receiving voltage almost directly

corresponds to the number outputted.

! " #! #" $! $" %! %" &! &" "!

'#(

###

#$(

#&%

#"*

#("

#*#

$!(

$$%

$%*

$""

!"#$%&$'()*+&,*-'.

+,--

+./01234,001

5-16/

78,923:1;/<-=::

>==?

>-,@?237-=::9

/+0'&12)34(5'&6578

!"#$%&12)34(5'

Figure 2.2.1. AX-S1 sensor model

Data was collected by varying the distance and materials of the surface. Each distance

was measured ten times and each point was plotted, as shown in Figure 2.2.1. The type of

surface aﬀects the resulting distance measurement. Given the same distances, the values

diﬀer somewhat based on the color or reﬂection of the obstacle. It is clear from the ﬁgure

2. 2D SCANNING OPTIONS 14

that as the obstacle gets glossier and darker in color, the value the sensor returns for the

particular distance decreases. For example, at a distance of 25 cm away from the module,

the values read are just under 15, just over 15 and 30, whereas at 11cm away, the values

are 47,47, and 118. Comparison of these two distances shows just how much noise there

is in the sensor.

2.3 Laser Range Scanner

Contrary to the sensor module, the laser range scanner was built to accomplish only the

speciﬁc task of measuring distances, and so oﬀers a much cleaner output. The laser used

here is able to do a 240

◦

scan of a plane in 100 milliseconds. Calibration for the mea-

surement and conversion of the actual distance versus the distance read by the laser was

based on the midpoint of the range it looks at - the point directly in front of the laser,

eﬀectively turing the 2D laser into a 1D sensor. Measurement and data collection for the

calibration was done in the same way as the laser - each distance was measured ten times,

recorded, and plotted. As shown in ﬁgure 2.3, even given the diﬀerent surfaces of varying

colors and gloss to skew the outcomes, the readings stayed fairly consistent. There was

some inconsistency in the readings that increased as the object moved farther away from

the source, but it stayed within a fairly small range.

2. 2D SCANNING OPTIONS 15

! " #! #" $! $" %! %" &! &" "!

#!!

#"!

$!!

$"!

%!!

%"!

&!!

&"!

"!!

""!

!"#$%&'$(#)%&*)+$,

'())

'*+,-./0(,,-

1)-2+

34(5./6-7+8)966

:99 ;

:)(<;./3)9665

-%.$&+/#-"(0 $&1023

,"#$%&%$"+/(4

Figure 2.3.1. Laser sensor model

3D Scanner Platform

3.1 Hardware

3.1.1 Laser Rig

Figure 3.1.1. Laser Rig: Laser attached to control module via a servo

The Laser Rig consists of four parts. The ﬁrst is the 2D laser range ﬁnder itself. It is

what actually collects the data in a plane. The second is the servo which allows tilting

3. 3D SCANNER PLATFORM 17

of the laser. The third is the connector piece between the two previous parts. The plate

attaches the laser to the servo allowing it to tilt. Lastly, the CM-5 is a controller that

allows for the control and communication of the servo with the computer.

3.1.2 Bioloid Overview

The Bioloid Kit is a robot kit designed to allow users to assemble robots to accomplish

almost any task. The kit allows for alteration of its physical and behavioral patterns much

in the same way as Lego or Erector kits do. It allows for pieces to be put together with

only nuts and bolts to produce a wide variety of robots. The Bioloid kit uses a series of

serially connected servos and sensor modules all connected to a central controller that

sends and receives information.

3.1.3 Bioloid Controller

Figure 3.1.2. CM-5

The CM-5 is the controller module and the brain of the whole kit. The CM-5 comes with

diﬀerent modes, buttons, and LEDs for user interaction. Communication between the CM-

5 and a computer is accomplished with a serial cable, but can also be achieved with a USB

adapter. Once the program is loaded, the CM-5 talks to the servos and sensor modules

3. 3D SCANNER PLATFORM 18

with serial cables that can be daisy-linked together using anything from one to three buses.

The module comes with three diﬀerent modes, each serving a unique function. The play

mode is used to run the residing program once it has been downloaded. The program mode

allows for the CM-5 to communicate with the computer as a motion editor. Lastly, the

manage mode is used to verify and run programs on the ﬂy. It allows for a connection with

a computer through which code can be directly executed and sent to the CM-5 without

downloading.

There are also four buttons of the module. Each can be programmed to perform a

certain task, as desired by the user. Lastly, there are three state indicating LEDs. Each is

responsible for showing the user what the module is doing, similar to the status LED on

a desktop.

3.1.4 Bioloid Servos

Figure 3.1.3. Left: Diﬀerent sides of the servo, Right: Servos serially linked to the CM-5

Motion is provided by the AX-12+ servos that are serially controlled. Whenever there is

an action sent to a particular servo, feedback is provided about the relevant information.

Some of these characteristics include continuous rotation, goal position, moving speed,

3. 3D SCANNER PLATFORM 19

temperature, voltage, and LED. The LED is used mostly for the purposes of debugging

– it can be turned on and oﬀ, but has no actual functionality. Rotation is set either as

an endless turn or it is set to a desired goal location. With the latter, the servo has 300

◦

of rotation and corresponds to a value range of 0 to 1023. The center resides at position

value 512 and is considered to be at 150

◦

. If it is the case that the angle limit for both

directions is set to 0, then it is possible to implement an endless turn mode. This is only

possible if the speed is set without a limit. This allows for the use of a continuous rotating

wheel. Temperature and voltage are both used as safety methods, and if either exceeds

the preset limits, the servo shuts oﬀ.

3. 3D SCANNER PLATFORM 20

DYNAMIXEL

AX-12

Goal Speed Setting

BIT 15~11 10 9876543210

Value 0 Turn Direction Speed Value

Turn Direction = 0 : CCW Direction Turn, Load Direction = 1: CW Direction Turn

Range Each data has a valid minimum and maximum values. Write instructions made outside

of these valid ranges will return an error. The following table summarizes the data range

for each register. 16 bit data registers are indicated with two bytes (L) and (H). Both

bytes need to be written at the same time as one instruction packet.

Write

Address

Writing Item

Length

(bytes)

Min Max

3(0X03) ID 1 0

253(0xfd)

4(0X04) Baud Rate 1 0

254(0xfe)

5(0X05) Return Delay Time 1 0

254(0xfe)

6(0X06) CW Angle Limit 2 0

1023(0x3ff)

8(0X08) CCW Angle Limit 2 0

1023(0x3ff)

11(0X0B) the Highest Limit Temperature 1 0

150(0x96)

12(0X0C) the Lowest Limit Voltage 1 50(0x32)

250(0xfa)

13(0X0D) the Highest Limit Voltage 1 50(0x32)

250(0xfa)

14(0X0E) Max Torque 2 0

1023(0x3ff)

16(0X10) Status Return Level 1 0

17(0X11) Alarm LED 1 0

127(0x7f)

18(0X12) Alarm Shutdown 1 0

127(0x7f)

19(0X13) (Reserved) 1 0

24(0X18) Torque Enable 1 0

25(0X19) LED 1 0

26(0X1A) CW Compliance Margin 1 0

254(0xfe)

27(0X1B) CCW Compliance Margin 1 0

254(0xfe)

28(0X1C) CW Compliance Slope 1 1

254(0xfe)

29(0X1D) CCW Compliance Slope 1 1

254(0xfe)

30(0X1E) Goal Position 2 0

1023(0x3ff)

32(0X20) Moving Speed 2 0

1023(0x3ff)

34(0X22) Torque Limit 2 0

1023(0x3ff)

44(0X2C) Registered Instruction 1 0

47(0X2F) Lock 1 1

48(0X30) Punch 2 0

1023(0x3ff)

[Control Table Data Range and Length for Writing]

Figure 3.1.4. Control table of data ranges [2]

3.1.5 Bioloid Sensor

The AX-S1 module provides of a variety of sensors. It has functions for sensing distance,

brightness, remote control infrared, heat, and sound. The module can also generate any

combination and sequence of 52 pitches of sound.

Both distance and brightness can be measured from three of the sides of the sensor,

whereas sound and remote control reception are done from only one side. Both brightness

and distance are measured as a value from 0 to 255. The distance sensor can provide close

3. 3D SCANNER PLATFORM 21

Figure 3.1.5. Top: Sensor module, Bottom: Close and long range sensing [1]

and long range sensing. The short range oﬀers a more precise value at close proximity,

while the long range allows for sensing farther away.

3.1.6 Hokuyo Laser

Laser range ﬁnders are extensively used in object recognition and modeling. The laser used

in this project was a $1,200 Hokuyo laser range ﬁnder. It uses an infrared laser and cal-

culates the distance based on the phase shift. It uses a rotating mirror to do the scanning

in a plane. It is able to scan 240

◦

, and has a range of about 20 centimeters to upwards of

5 meters, but is guaranteed to give an accurate measurement for ranges up to 4 meters.

After a planar scan, the laser returns an array of values. As shown in Figure 3.1.6, there

are over seven-hundred steps, with the middle value being step 384. The laser obtains the

3. 3D SCANNER PLATFORM 22

TITLE

DRAWING

NO.

COMMUNICATION PROTOCOL

SPECIFICATION - URG SERIES

C-42-3320-

7/7

Angular detection ran ge of t he sen sor is 240 degree .

Angular resolut ion per step is 360 degr ee/1024 = 0.3515625 degree. St ep n u m bers “0”,

“38 4 ” a n d ‘7 6 8” im pli e s -1 3 5 d e gr ee , 0 d egr ee and +135 degree respectively taking the front

of the sensor as a reference point (Figur e below).

Figure: Top View

Sensor measures the distance in the range 20 mm to 4094 mm. If the distance is less

than 20 mm, data will have error code.

Error Code Error Type

0 Possibility of detected object is at 22m

1 Reflected light has low intensity

2 Reflected light has low intensity

3 Reflected light has low intensity

4 Reflected light has low intensity

5 Reflected light has low intensity

6 Possibility of detected object is at 5.7m

7 Distance data on the preceding and succeeding steps have errors

8 Others

9 The sa m e step h a d err or in the last t wo sca n

10 Others

11 Others

12 Others

13 Others

14 Others

15 Others

16 Possibility of detected object is in the range 4096mm

17 Others

18 Unspecified

19 Non-Measurable Distance

DEAD ZONE

FRONT

Step0

Step768

Step384

Direction of Rotation

Measurable Range of

URG-X002

Step7

Step

Figure 3.1.6. Left: Laser range scanner, Right: Measurable range of the scanner

distance between itself and the ﬁrst obstacle it ﬁnds, places that value in the step it is in

and moved to the next step to repeat the process.

3.1.7 Connector

Figure 3.1.7. Plate connector

Both the Bioloid Kit and the laser have screw mounts. The kit uses a series of tiny nuts

and bolts of ranging lengths to hold two pieces together. The laser uses two screws on the

opposite sides of its base in order to be attached onto a plate of some sort. To connect

3. 3D SCANNER PLATFORM 23

the two pieces, a joining connector plate needed to be manufactured. A piece of custom

machine shopped plexiglass was used. Six holes were precision drilled, two for the laser

and four to attach to the kit. Because the laser and kit were on opposite sides of each

other, the four holes needed countersinks to keep the plate ﬂush with the connector.

3.2 Software

3.2.1 Bioloid Standard Applications

The Bioloid kit comes with three standard computer programs used to create programs

for robots. Each one is unique to it’s main function and purpose: The Behavior Control

Programmer (BCP), the Motion Editor (ME), and the Robot Terminal (RT). The BCP

is a graphical user interface to create programs. It uses icons that can be placed, one

below the other, to create a list of commands to be executed. Similar to any programming

language, there are loops, branches, and conditions. The ME allows for low-level commu-

nication. It controls individual servos to create actions that are very precise. The actions

are presented in a 3D interface and can be accessed by the BCP. The RT is a lower-level

communication application that talks directly to the servo through the CM-5. The Robot

Terminal is similar to the terminal application in a Mac and the Command Prompt on

Windows, but communicates only with the Bioloid.

3.2.2 Python Libraries

Both the Bioloid kit and the laser scanner come with their own libraries. The kit comes

with three software applications and is programmed in C. With the help of Doug Blank

at Bryn Mawr College, communication with the CM-5 in Python was established. Blank

provided a skeleton that gave basic functionality to the servos.

3. 3D SCANNER PLATFORM 24

Python

PySerial

Library

Gnuplot

Library

Command Module

URG-04LX

Figure 3.2.1. System-Level Block Diagram

A Python library that communicates with the laser was found on github [1]. Mainly,

this library is used to read the array of distances measured in a scan. Other interesting

functions include the ability to turn the laser on and oﬀ, access various information such

as version and parameters.

3D Model Building

4.1 Algorithm

Stitching together a series of 2D scans by far the most interesting and diﬃcult part of

the project. The raw information was provided by two sources that don’t mix particularly

well. The problem was that the data was captured in diﬀerent coordinate frames, and

needed to be integrated into a common frame. Looking at the laser alone, it provided

an array of distances. The azimuth, the angle between two points at which the distance

is measured, was known, and therefore a 2D polar plot could be made in that plane.

With the addition of a servo, a third dimension was given. Given the three dimensions,

the points exist in spherical coordinates. Conversion to Cartesian coordinates proved to

be a big challenge. After a series of tries of diﬀerent algorithms, a new approach was taken.

The servo tilts in the X-Z plane. If the laser is ﬂat it scans in the X-Y plane, and has no

variation among any of the points in the Z-plane. If, however, it is facing up, then it scans

in the Y-Z plane, and has no variation in the X-plane. Given this, X and Z-planes are

opposites of each other. With the rotation of the servo, the angle was ignored and labeled

4. 3D MODEL BUILDING 26

the ”B-axis.” This B-axis is thought as a plane that rotated anywhere about the Y-axis If

the laser captures a scan in this B-Y plane, it will simply create a polar plot. Conversion

from this ”B-Y plot” into an X-Y-Z plot, was accomplished using the following algorithm:

while (servo moving):

tilt = angle of B-axis above the X-axis

for i in length of all the distances in a planar scan:

x = i * [cos(i) * (azimuth angle)] * [sin (tilt)]

y = i * [sin(i) * (azimuth angle)]

z = i * [cos(i) * (azimuth angle)] * [cos (tilt)]

Once the conversion is complete, it is then visualized in two diﬀerent programs. Gnu-

plot, which is integrated into Python, reads the converted ﬁle and plots all the points. The

points are colored based on their distance in the X-axis. The user can rotate the plot in

any direction they wish. The second visualization is done in Processing, a Java-like pro-

gramming language that plots all the data and rotates the captured environment around

the Z-axis.

4. 3D MODEL BUILDING 27

4.2 Early Results and Problems

Figure 4.2.1. Model standing in the middle of the room

The scans above (Figure 4.2.1) show the range scanner in the corner of a room pointing

at a person standing in the middle of the room with their arms out, perpendicular to their

body. This scan was done using an early algorithm. This scan conﬁrms that the algorithm

was roughly correct as it presents a few similarities to reality. Having the individual put

their arms out shows that the resemblance is fairly accurate. The slant in the ceiling is not

the poor construction of the house but rather an early stage in the algorithm. The early

stage of the algorithm was missing components that would correctly convert the frames

into the X-Y-Z axes.

4. 3D MODEL BUILDING 28

Figure 4.2.2. Far wall and corner of a room

With some basic orientation established, improvement came next. Figure 4.2.2 shows

an attempt to improve on the straightness of ﬂat surfaces. Some of the extremeness of the

sagged ceiling is gone. The geometry of the window, which appears as a hole in the scan

on the left side of the room, seems to be near rectangular but almost perfect with the

shape of the room. The break between the room and the scan on the right is created by

a closet whose back wall is outside of the range of the scanner.

4. 3D MODEL BUILDING 29

Figure 4.2.3. Corner of a room leading to two directions

The recreation of the environment shown in Figure 4.2.3 presents a curious and some-

what complicated scan. The early algorithm used to create this model shows the piano

near the corner of a room. That corner also leads to another room via an archway on one

side and into the hallway on the other. The points protruding from the right side of the

wall represent a piano. The scan was taken from the ground with a fairly big rise between

each scan, and so the piano does not have many points to accurately portray it.

4. 3D MODEL BUILDING 30

Figure 4.2.4. Corner of the sun room

In Figure 4.2.4, the walls became more of a challenge and hence more engaging. There

is a corner that protrudes in the middle of the scan. To the right is a wall and a window.

Even further to the right, a part of a nook with a table starts appearing. Looking closely,

it is possible to notice that the part where the edge of the table appears (see later for the

picture), the scans shows to be somewhat protruded from the wall.

4.3 Finalized Algorithm

The following scans use the ﬁnalized algorithm. The environments are each scanned at

four diﬀerent speeds. Each scan is visualized and two diﬀerent views are shown to better

4. 3D MODEL BUILDING 31

represent the scan. The speeds determine how fast the servo moves, which in turn deter-

mines how much data is collected. Each subsequent speed is roughly twice as fast as the

previous and so collects almost half as much data. At the slowest speed, the amount of

data collected is quite large. The average ﬁle size is about 4.5MB. Looking at the number

of points, the most of the environments hover at about 100,000 points with the larger ﬁles

being just over 130,000 points.

Figure 4.3.1. Living Room Panorama

The scans, as shown in Figures 4.4.1 - 4.4.4 capture the environment pictured above

(Figure 4.3.1). Each pair shows the same environment as the servo speed increases. Com-

paring each clearly shows how much information is lost by doubling the speed. At speed

10 it is near impossible to even tell where the windows are. The scan shows an armchair

in the middle of the room and a sofa to it’s left. There is a window below the couch as

well as another to the right of the armchair. Opposite the couch is an archway. The scan

oﬀers a variety of interesting objects of various diﬃculty. The windows, walls, and ceiling

4. 3D MODEL BUILDING 32

conﬁrm the geometry and correctness of the coordinate conversion. The armchair is by

far the most fascinating and complex part of the environment as it has curved edges and

uneven surfaces.

Figure 4.3.2. Model standing in the middle of the room

The laser range ﬁnder captures a person in the middle of the room. The model is stand-

ing in the same place and has her hands out just as an earlier capture had. This scan

is taken to show the improvement over the early algorithm. Figure 4.3.6 visualizes the

capture in Gnuplot so that the two can be compared. This capture is done using a servo

speed of two. As ﬁgures 4.4.5 - 4.4.8 show, the ceilings and walls are all ﬂat, and the model

is set away from all the walls.

4. 3D MODEL BUILDING 33

Figure 4.3.3. Scan of the model standing in the middle of the room using the ﬁnalized

algorithm

Figure 4.3.4. Weis Cinema

This environment captures the walkway and chairs in Weis Cinema. As the visualizations

show in ﬁgures 4.4.9 - 4.4.12, the chairs line up perfectly. Similar to other environments,

the density of the scans reduces and detail is lost as the speed at which the servo moves

increases.

4. 3D MODEL BUILDING 34

Figure 4.3.5. Stairs

The stairs data set, as pictured in Figure 4.3.5, is the most impressive scan in the

project. As shown in Figures 4.4.13 through 4.4.16, the environment captures the stairs

going down, and a room at the landing. Like previous scans, this shows four speeds of the

servo and two views of each speed. Similarly, it also shows that as the speed increases, the

number of points decreases. A noteworthy observation about this environment is that parts

of it arguably look better at a speed of 2. At the slowest speed, the top few stairs look busy

and the start to blend with each other. Overall though, this environment does not need

all the points - the risers between each step can be blank as long as the step itself is visible.

4.4 Scans

4. 3D MODEL BUILDING 35

Figure 4.4.1. Armchair 10

Figure 4.4.2. Armchair 4

4. 3D MODEL BUILDING 36

Figure 4.4.3. Armchair speed 2

Figure 4.4.4. Armchair speed 1

4. 3D MODEL BUILDING 37

Figure 4.4.5. Model speed 10

Figure 4.4.6. Model speed 4

4. 3D MODEL BUILDING 38

Figure 4.4.7. Model speed 2

Figure 4.4.8. Model speed 1

4. 3D MODEL BUILDING 39

Figure 4.4.9. Weis speed 10

Figure 4.4.10. Weis speed 4

4. 3D MODEL BUILDING 40

Figure 4.4.11. Weis speed 2

Figure 4.4.12. Weis speed 1

4. 3D MODEL BUILDING 41

Figure 4.4.13. stairs Speed 10

4. 3D MODEL BUILDING 42

Figure 4.4.14. stairs Speed 2

4. 3D MODEL BUILDING 43

Figure 4.4.15. stairs Speed 2

4. 3D MODEL BUILDING 44

Figure 4.4.16. stairs Speed 1

Discussion and future work

5.1 Attachment

The Laser Rig can be attached to a variety of robots, for example an exploration rover.

It can move around and scan various environments as it explores. As the rover explores,

it will create 3D environments that are much bigger than a single scan. At the end of

the exploration, the environment will be visualized to create a complete map. Another

application is a ﬂying robot. The laser rig can function in the same way, but return a

more complete visualization from diﬀerent elevations.

5.2 Algorithm Improvement

A further improvement of the algorithm is to create surfaces out of the point clouds.

Multiple points can be grouped together to create a ﬂat surface that better represent the

environment. Multiple surfaces can be visualized together to better depict an environment.

A further implementation is to sync two captures into a single one via an Iterative

Closest Point (ICP) algorithm.This would correct the inaccuracies in the measurements

5. DISCUSSION AND FUTURE WORK 46

of the tilt angles. The captures are taken of the same space from two angles. This gives

a more detailed environment as it provides depth to objects. The ICP algorithm takes

point clouds and compares individual points in relation to one another to be able to sync

captures. An example is to scan a box from the front and again from the corner. The ICP

algorithm uses the two captures to create a box that represents the front of the box and

it’s side. Each consecutive capture is added to the ﬁnal scan to create a complete box.

This same idea can be used on more complex objects to better depict an environment.

Bibliography

[1] Hokuyo-URG Laser Range Sensor in Python, Access Date January 2011, https:

//github.com/nus/pyURG.

[2] Bioloid User’s Manual: Programming for Bioloid, Robotis Co., LTD, Bucheon, 2007.

[3] AX-12 User’s Manual, Access Date January 2011, http://www.robosavvy.com/

RoboSavvyPages/Support/.../AX-12(english).pdf.

[4] AX-S1 User’s Manual, Access Date January 2011, http://www.robosavvy.com/

RoboSavvyPages/Support/.../AX-S1(english).pdf.

[5] URG-04LX-UG01 Speciﬁcations, Access Date January 2011, http://www.

hokuyo-aut.jp/02sensor/07scanner/download/data/URG-04LX_UG01_spec.pdf.

[6] URG Series Communication Protocol Speciﬁcation, Access Date January 2011,

http://http://www.hokuyo-aut.jp/02sensor/07scanner/download/data/URG_

SCIP11.pdf.

[7] Francois Blais, Review of 20 Years of Range Sensor Development, Journal of Elec-

tronic Imaging January (2004), 231-240.

[8] Yang Chen and Gerand Medioni, Object Modeling by Registration of Multiple Range

Images, Robotics and Automation, 1991. Proceedings., 1991 IEEE International Con-

ference on Vol. 3 (1991), 2724 - 2729.

[9] Anthony Dearden and Yinnis Demiris, Learning Forward Models for Robots, in Pro-

ceedings of IJCAI (2005), 1440–1445.

[10] Dirk Haehnel Alensandr V. Segal and Sebastian Thrun, Generalized-ICP, In Proceed-

ings of Robotics: Science and Systems June Issue (2009).

[11] Pletro Morasso and Vittorio Sanguineti, Self-Organizing Body Schema for Motor

Planning, Journal of Motor Behavior Vol. 27 (1995), 52-66.

[12] Jurgen Sturm Christian Plagemann and Wolfram Burgard, Adaptive Body Scheme

Models for Robust Robotic Manipulation, Robotics: Science and Systems (RSS) June

(2008).